#include <ros/ros.h>
#include <nav_msgs/OccupancyGrid.h>
#include <sensor_msgs/LaserScan.h>
#include <cmath>
#include <vector>

class OccupancyGridToLaserScan
{
public:
    OccupancyGridToLaserScan(ros::NodeHandle &nh) : nh_(nh)
    {
        ros::NodeHandle pnh_("~");
        // 从参数服务器获取话题名称
        std::string ocpg_topic, laserscan_topic;
        pnh_.getParam("ocpg_topic", ocpg_topic);
        pnh_.getParam("laserscan_topic", laserscan_topic);

        // 订阅OccupancyGrid话题
        sub_ = nh_.subscribe(ocpg_topic, 1, &OccupancyGridToLaserScan::mapCallback, this);

        // 发布LaserScan话题
        pub_ = nh_.advertise<sensor_msgs::LaserScan>(laserscan_topic, 1);
    }

private:
    ros::NodeHandle nh_;
    ros::Subscriber sub_;
    ros::Publisher pub_;

    void mapCallback(const nav_msgs::OccupancyGrid::ConstPtr &msg)
    {
        // 创建LaserScan消息
        sensor_msgs::LaserScan scan;
        scan.header = msg->header;
        scan.header.frame_id = msg->header.frame_id;

        // 设置激光扫描参数（根据需求调整）
        float n = 5;

        float angle_min = -M_PI;              // 起始角度
        float angle_max = M_PI;               // 结束角度
        float angle_increment =  M_PI / 180.0 / n; // 每度 n 个点
        float range_min = 0.0;
        float range_max = 10.0; // 最大探测距离

        scan.angle_min = angle_min;
        scan.angle_max = angle_max;
        scan.angle_increment = angle_increment;
        scan.time_increment = 0.0;
        scan.range_min = range_min;
        scan.range_max = range_max;
        scan.ranges.resize(static_cast<int>((angle_max - angle_min) / angle_increment) + 1, 0.0);

        // 获取地图信息
        int width = msg->info.width;
        int height = msg->info.height;
        float resolution = msg->info.resolution;
        float origin_x = msg->info.origin.position.x;
        float origin_y = msg->info.origin.position.y;

        // 假设机器人位于地图原点（0,0）
        int robot_x = -origin_x / resolution;
        int robot_y = -origin_y / resolution;

        // 遍历每个角度，模拟激光扫描
        for (size_t i = 0; i < scan.ranges.size(); ++i)
        {
            float angle = angle_min + i * angle_increment;
            float min_range = range_max;

            // 射线追踪：从机器人位置沿angle方向搜索最近障碍物
            for (float r = 0.0; r < range_max; r += resolution)
            {
                int x = robot_x + r * cos(angle) / resolution;
                int y = robot_y + r * sin(angle) / resolution;

                if (x < 0 || x >= width || y < 0 || y >= height)
                    break;

                int index = y * width + x;
                if (msg->data[index] > 50)
                { // 占用（值>50）
                    min_range = r;
                    break;
                }
            }

            scan.ranges[i] = min_range;
        }

        // 发布LaserScan消息
        pub_.publish(scan);
    }
};

int main(int argc, char **argv)
{
    ros::init(argc, argv, "ocpg_to_laserscan");
    ros::NodeHandle nh;
    OccupancyGridToLaserScan converter(nh);
    ros::spin();
    return 0;
}